/*
 * To change this license header, choose License Headers in Project Properties.
 * To change this template file, choose Tools | Templates
 * and open the template in the editor.
 */
package eu.bopet.jocadv.core.constraints;

import eu.bopet.jocadv.core.Constraint;
import eu.bopet.jocadv.core.Differentiable;
import eu.bopet.jocadv.core.Parameter;
import eu.bopet.jocadv.core.datum.Point;
import java.io.Serializable;

/**
 * The class represents geometric constraint of distance between two points
 *
 * @author bocskapeter
 */
public class PointToPointDistance extends Constraint implements Serializable, Differentiable {

    /**
     * Default constructor
     *
     * @param name
     * @param point1
     * @param point2
     * @param distance
     */
    public PointToPointDistance(String name, Point point1, Point point2, double distance) {
        super(name);
        this.addToElementList(point1);
        this.addToElementList(point2);
        Parameter parameter = new Parameter("Parameter");
        parameter.addParameterToList(distance);
        this.setParameter(parameter);
    }

    /**
     * Gives the value of the equation at the given point<br>
     * Variables: point1: x1, y1, z1 ; point2: x2, y2, z2 and distance: d.<br>
     * Equation: (x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1) + (z2 - z1) * (z2
     * - z1) - d * d
     *
     * @param p - point
     * @return double value
     */
    @Override
    public double getVaue(double[] p) {
        double distance = this.getParameter().getParameterList().get(0);
        return (p[3] - p[0]) * (p[3] - p[0]) + (p[4] - p[1]) * (p[4] - p[1]) + (p[5] - p[2]) * (p[5] - p[2]) - (distance * distance);

    }

    /**
     * Gives the values of the partial derivative at the given point<br>
     * Variables: point1: x1, y1, z1 ; point2: x2, y2, z2 and distance: d.<br>
     * <p>
     * Partial derivatives: <br><br>
     * point1: <br>
     * dx1: 2 * x1 - 2 * x2 <br>
     * dy1: 2 * y1 - 2 * y2 <br>
     * dz1: 2 * z1 - 2 * z2 <br><br>
     * point2: <br>
     * dx2: 2 * x2 - 2 * x1 <br>
     * dy2: 2 * y2 - 2 * y1 <br>
     * dz2: 2 * z2 - 2 * z1 <br><br>
     * distance: <br>
     * dd: 2 * d <br></p>
     *
     * @param p point
     * @return array of double values
     */
    @Override
    public double[][] getDerivatives(double[] p) {
        double distance = this.getParameter().getParameterList().get(0);
        double[][] jacobi = new double[3][3];

        //dx1:
        jacobi[0][0] = 2 * p[0] - 2 * p[3];
        //dy1:
        jacobi[0][1] = 2 * p[1] - 2 * p[4];
        //dz1:
        jacobi[0][2] = 2 * p[2] - 2 * p[5];

        //dx2:
        jacobi[1][0] = 2 * p[3] - 2 * p[0];
        //dy2:
        jacobi[1][1] = 2 * p[4] - 2 * p[1];
        //dz2:
        jacobi[1][2] = 2 * p[5] - 2 * p[2];

        //dd:
        jacobi[2][0] = 2 * distance;

        return jacobi;
    }

}
